import java.awt.*;
import java.awt.geom.*;
import java.awt.image.BufferedImage;
import java.io.*;
import java.util.*;
import java.util.regex.Pattern;
import javax.swing.*;

public class Map extends JPanel {
	/**
	 * @param args
	 */
	public int map_size_height;
	public int map_size_width;
	private java.util.List<Ellipse2D> walls = new LinkedList<Ellipse2D>();
	private java.util.List<Line> floor = new LinkedList<Line>();
	private java.util.List<Line> paths = new LinkedList<Line>();
	private Point robot_pos;
	private double robot_orientation;
	private int extreme;

	public Map() {
		this.Set_Map_Size(50, 50);
		setBackground(Color.white);
		robot_pos = new Point(0, 0);
		robot_orientation = 0.0;
	}

	// Yeild_Map() returns a a buffered image, a map representation of the
	// environment
	public BufferedImage Yeild_Map() {
		
		int w = this.map_size_width;
		int h = this.map_size_height;
		
//		int w = this.getWidth();
//		int h = this.getHeight();
		BufferedImage bi = new BufferedImage(w, h, BufferedImage.TYPE_INT_RGB);
		Graphics2D g = bi.createGraphics();
		//g.scale(800.0/((double)w), 600.0/((double)h));
		this.paint(g);
		
		return bi;
	}

	// Update is called to update the environment
	// ex.
	// mapObj.Update("<r2s><scan n=\"14\" n1=\"14\" n2=\"25\" n3=\"25\"/></r2s>")
	public void Update(XMLHandler xml) {

		for (int i = 0; i < xml.getCommand().size(); i++) {

			// Move
			if (xml.isMove(i)) {
				System.out.println("move");
				this.Update_Env(true, this.robot_orientation,
						xml.getValue(i, "real"));
				System.out.println(" real: " + String.valueOf(xml.getValue(i, "real")));

				// Rotate
			} else if (xml.isRotate(i)) {

				System.out.println("rotate");
				robot_orientation = (robot_orientation + xml
						.getValue(i, "real")) % 360;
				System.out.println(" real: " + String.valueOf(xml.getValue(i, "real")));

				// scan
			} else if (xml.isScan(i)) {

				System.out.println("scan");
				int n = xml.getValue(i, "n");
				double angle = robot_orientation;
				double measure = 0.0;
				
				System.out.println(" n: " + String.valueOf(n));
			
				for (int j = 0; j < n; j++) {
					measure = (double) xml.getValue(i, "n" + String.valueOf(j));
					System.out.println(" n" + String.valueOf(j) + ": " + String.valueOf(measure) + " angle: " + String.valueOf(angle));
					this.Update_Env(false, angle, measure);
					angle = (angle + (360 / n) % 360);
				}
				
				
			}
			
		}

		// use XML-handler here? or write nicer regexs.
		// if (Pattern.matches(".*scan.*", xmlCommand)) {
		// double angle = this.robot_orientation;
		// String digits = xmlCommand.replaceAll("\\D+", ":");
		// String str[] = digits.split(":");
		// for (int i = 2; i < str.length; i += 2) {
		// this.Update_Env(false, angle, Double.parseDouble(str[i]));
		// angle = (angle + (360 / ((str.length - 1) / 2)) % 360);
		// }
		// } else if (Pattern.matches(".*move.*", xmlCommand)) {
		// String digits = xmlCommand.replaceAll("\\D+", ":");
		// String str[] = digits.split(":");
		// this.Update_Env(true, this.robot_orientation,
		// Double.parseDouble(str[2]));
		// } else if (Pattern.matches(".*rotate.*", xmlCommand)) {
		// String digits = xmlCommand.replaceAll("\\D+", ":");
		// String str[] = digits.split(":");
		// robot_orientation = (robot_orientation + Double.parseDouble(str[2]))
		// % 360;
		// }
		
		this.setSize(this.map_size_width, this.map_size_height);
		this.repaint();
	}

	private void Set_Map_Size(int h, int w) {
		this.map_size_height = h;
		this.map_size_width = w;
	}

	// path indicates if you want to draw a trajectory
	private void Update_Env(boolean path, double angle, double measure) {
		int x_end = (int) (Math.cos(Math.toRadians(angle)) * measure)
				+ this.robot_pos.x;
		int y_end = (int) (Math.sin(Math.toRadians(angle)) * measure)
				+ this.robot_pos.y;
		Point endPoint = new Point(x_end, y_end);
		double dist_origo = Math.sqrt(Math.pow(x_end, 2) + Math.pow(y_end, 2));
		// If endPoint is an extreme point, save it to determine map size
		if (dist_origo > extreme) {
			this.extreme = (int) dist_origo;
			this.Set_Map_Size(extreme * 2 + 20, extreme * 2 + 20);
		}
		// Add trajectory
		if (path) {
			Line newLine = new Line(this.robot_pos, endPoint);
			paths.add(newLine);
			robot_pos.setLocation(x_end, y_end);
		}
		// Add measuring distance with endpoint (wall)
		else if (!path /*&& measure < 15*/ && measure < 60) {
			Line newLine = new Line(this.robot_pos, endPoint);
			floor.add(newLine);
			int dot_size = 4;
			Ellipse2D newWall = new Ellipse2D.Double(x_end - dot_size / 2, y_end
					- dot_size / 2, dot_size, dot_size);
			walls.add(newWall);
		}
		// Add measuring distance without endpoint
		else if (!path && measure > 170) {
			Line newLine = new Line(this.robot_pos, endPoint);
			floor.add(newLine);
		}
	}
	
	private void align_Walls(){	
		int span = 20;
		for(int x = span; x < map_size_width; x += span){
			int average_x = 0;
			int nbr_points = 0;
			java.util.List<Integer> dots = new LinkedList<Integer>();
			for (Ellipse2D e : this.walls) {
				if(x - span < e.getX() && e.getX() < x){
					average_x += e.getX();
					nbr_points++;
					dots.add(walls.indexOf(e));
				}
			}
			if(nbr_points>0)
				average_x = average_x/nbr_points;
			for (Integer i : dots) {
				walls.set(i, new Ellipse2D.Double(average_x, walls.get(i).getY(), 
						walls.get(i).getWidth(), walls.get(i).getHeight()));
			}
		}
		for(int y = span; y < map_size_width; y += span){
			int average_y = 0;
			int nbr_points = 0;
			java.util.List<Integer> dots = new LinkedList<Integer>();
			for (Ellipse2D e : this.walls) {
				if(y - span < e.getY() && e.getY() < y){
					average_y += e.getY();
					nbr_points++;
					dots.add(walls.indexOf(e));
				}
			}
			if(nbr_points>0)
				average_y = average_y/nbr_points;
			for (Integer i : dots) {
				walls.set(i, new Ellipse2D.Double(walls.get(i).getX(), average_y,  
						walls.get(i).getWidth(), walls.get(i).getHeight()));
			}
		}
		
	}

	@Override
	public void paintComponent(Graphics g) {
		
		super.paintComponent(g);
		
		
		// Move origin to center of image
		g.translate(this.map_size_height / 2, this.map_size_width / 2);
		Graphics2D g2 = (Graphics2D) g;
		// Paint the environment
		g2.setStroke(new BasicStroke(4));
		g.setColor(new Color(80, 255, 135));
		for (Line l : floor) {
			g2.drawLine(l.p1.x, l.p1.y, l.p2.x, l.p2.y);
		}
		g.setColor(new Color(255, 255, 100));
		for (Line l : paths) {
			g2.drawLine(l.p1.x, l.p1.y, l.p2.x, l.p2.y);
		}
		g2.setPaint(new Color(255, 73, 73));
		//align_Walls();
		for (Shape s : this.walls) {
			g2.draw(s);
			g2.fill(s);
		}
		
		// Paint robot position
		g2.setPaint(Color.black);
		g2.drawOval(robot_pos.x, robot_pos.y, 2, 2);
		
		
	}

	public class Line implements Serializable {
		public Point p1, p2;

		public Line() {
			p1 = new Point();
			p2 = new Point();
		}

		public Line(Point a, Point b) {
			p1 = new Point(a);
			p2 = new Point(b);
		}
	}

}
